#pragma once

#include "../include/common.hpp"
#include "../include/detection.hpp"
#include "./track.cpp"
#include "./control.cpp"
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <algorithm> 

#define RACING_ALLOW    5
#define SAFETY_ADD      (COLSIMAGE/8)
#define DANGER_ADD      (COLSIMAGE/5)
#define RACING_EXIT     10
#define ENTER_WAIT      (ROWSIMAGE/4)
#define ENTER           (ROWSIMAGE/4)
#define EXIT_DELAY      (700)
#define CHANGE_ADDLINE  (0)
#define BIAS            (5)
#define WAIT_DELAY      (3000)

using namespace std;
using namespace cv;

class Racing 
{
private:
    int prop;
    int safety;
    int spy;
    int danger;


    //左边是0
    bool direction;

    //丢失计数
    int lose;

    //延时用的东西
    long startTime;
    long preTime;

    PredictResult last_vehicle;
    control bias;
public:
    //小车类别
    int vehicle_type;
    //小车步骤
    int step;
    enum STEP
    {
        EnterWait=0,
        Enter,
        ExitWait,
        Exit,
        Change,
        Wait
    };
    void init()
    {
        prop=0;safety=0;spy=0;danger=0;
        lose=0;
        step=0;
        last_vehicle.type=LABEL_NONE;
    }

    bool if_racing(track & track,vector<PredictResult>predict)
    {
        int i;
        PredictResult vehicle;
        for(i=0;i<predict.size();i++)
        {
            if(predict[i].type==LABEL_PROP){
                prop++;
                vehicle=predict[i];
                break;
            }else if(predict[i].type==LABEL_SAFETY){
                safety++;
                vehicle=predict[i];
                break;
            }else if(predict[i].type==LABEL_SPY){
                spy++;
                vehicle=predict[i];
                break;
            }else if(predict[i].type==LABEL_DANGER){
                danger++;
                vehicle=predict[i];
                break;
            }
        }

        if(safety>RACING_ALLOW)
        {
            vehicle_type=LABEL_SAFETY;
            init();
            direction_judgment(track,vehicle);
            last_vehicle=vehicle;
            return true;
        }
        if(spy>RACING_ALLOW)
        {
            vehicle_type=LABEL_SPY;
            init();
            direction_judgment(track,vehicle);
            last_vehicle=vehicle;
            return true;
        }
        if(danger>RACING_ALLOW)
        {
            vehicle_type=LABEL_DANGER;
            init();
            direction_judgment(track,vehicle);
            last_vehicle=vehicle;
            return true;
        }
        if(prop>RACING_ALLOW)
        {
            vehicle_type=LABEL_PROP;
            init();
            direction_judgment(track,vehicle);
            last_vehicle=vehicle;
            return true;
        }
        return false;
    }

    //判断小车靠做还是靠右   
    void direction_judgment(track & track,PredictResult predict)
    {
        if(predict.type==LABEL_NONE)
        {return;}
        int i;
        int sum_right=0;
        int sum_left=0;
        for(i=predict.y;i<(predict.y+predict.height);i++)
        {
            if(i>=ROWSIMAGE||i<0)
            {break;}
            sum_right+=track.r_border[i].y;
            sum_left+=track.l_border[i].y;
        }
        sum_left/=predict.height;
        sum_right/=predict.height;
        sum_left=abs((predict.x+(predict.width/2))-sum_left);
        sum_right=abs(sum_right-(predict.x+(predict.width/2)));
        if(sum_left<sum_right){     //靠左=0
            direction=false;
        }else{
            direction=true;
        }
        
    }
    //获得小车的对象
    PredictResult get_vehicle(vector<PredictResult> predict)
    {
        PredictResult vehicle;
        vehicle.type=LABEL_NONE;
        for(int i=0;i<predict.size();i++)
        {
            if(predict[i].type==vehicle_type)
            {
                vehicle = predict[i];
            }
        }
        return vehicle;
    }
    //补线
    void racing_add_line(track & track,PredictResult vehicle)
    {
        PredictResult add_vehicle=vehicle;
        if(add_vehicle.type==LABEL_NONE)
        {add_vehicle=last_vehicle;}
        if(add_vehicle.type==LABEL_NONE)
        {return;}
        //左边；direction=0;补左边线；找右边
        if(direction==false)
        {
            int left_add = add_vehicle.width+SAFETY_ADD;//补线的位置;向右扩张，加
            for(int i=0;i<ROWSIMAGE;i++)//补线
            {
                track.l_border[i].y+=left_add;
            }
        }
        else
        {
            int right_add = add_vehicle.width+SAFETY_ADD;//补线的位置
            for(int i=0;i<ROWSIMAGE;i++)//补线
            {
                track.r_border[i].y-=right_add;
            }
        }
    }
    void spy_racing_add_line(track & track)
    {
        if(direction==false)//靠左补右边线
        {
            for(int i=0;i<ROWSIMAGE;i++)
            {
                track.r_border[i].y=(COLSIMAGE/2+CHANGE_ADDLINE);
            }
        }
        else
        {
            for(int i=0;i<ROWSIMAGE;i++)//补线
            {
                track.l_border[i].y=(COLSIMAGE/2+CHANGE_ADDLINE);
            }
        }
    }
    void danger_racing_add_line(track & track)
    {
        //左边；direction=0;补左边线；找右边
        if(direction==false)
        {
            int left_add = DANGER_ADD;//补线的位置;向右扩张，加
            for(int i=0;i<ROWSIMAGE;i++)//补线
            {
                track.r_border[i].y-=left_add;
            }
        }
        else
        {
            int right_add = DANGER_ADD;//补线的位置
            for(int i=0;i<ROWSIMAGE;i++)//补线
            {
                track.l_border[i].y+=right_add;
            }
        }
    }
    bool process(track & track ,vector<PredictResult> & predict)
    {
        if(vehicle_type==LABEL_PROP)
        {
            return prop_peocess(track,predict);
        }else if(vehicle_type==LABEL_SAFETY)
        {
            return safety_peocess(track,predict);
        }else if(vehicle_type==LABEL_SPY)
        {
            return spy_peocess(track,predict);
        }else if(vehicle_type==LABEL_DANGER)
        {
            return danger_peocess(track,predict);
        }
    }
    //处理prop
    int prop_peocess(track & track ,vector<PredictResult> & predict)//什么不做
    {
        PredictResult vehicle = get_vehicle(predict);
        //判断小车方向
        direction_judgment(track,vehicle);
        //降速

        //继续判断小车的类型
        if_racing(track,predict);
    }
    //safety    绕道
    int safety_peocess(track & track ,vector<PredictResult> & predict)
    {
        PredictResult vehicle;
        vehicle.type=LABEL_NONE;
        switch(step)
        {
        //判断小车是否到达目标距离
        case STEP::EnterWait:
            vehicle = get_vehicle(predict);
            if((vehicle.y+vehicle.height)>ENTER_WAIT)
            {
                step=STEP::Enter;
            }
        break;
        //（1）根据小车的位置补线（2）判断小车是否到达一定位置
        case STEP::Enter:
            vehicle = get_vehicle(predict);
            //补线
            racing_add_line(track,vehicle);
            if((vehicle.x+vehicle.height)>ENTER)//
            {
                step=STEP::ExitWait;      
            }
            if(vehicle.type!=LABEL_NONE){last_vehicle=vehicle;} 
        break;
        //（1）补线（2）如果小车消失多帧则判断结束（3）判断结束之后开始定时
        case STEP::ExitWait:
            vehicle = get_vehicle(predict);
            //补线
            racing_add_line(track,vehicle);         
            if(vehicle.type==LABEL_NONE)
            {
                //等于这个就相当于丢图
                lose++;
                if(lose>RACING_EXIT)
                {
                    preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                    startTime=preTime;
                    step=STEP::Exit;
                    break;
                }
            }
            if(vehicle.type!=LABEL_NONE){last_vehicle=vehicle;}       
        break;
        //（1）补线（2）当定时到达目标的时候结束
        case STEP::Exit:
            racing_add_line(track,last_vehicle); 
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            if((startTime - preTime)>EXIT_DELAY)
            {
                init();
                return true;
            }
        break;
        }
        return false;
    }
    //spy       截停
    int spy_peocess(track & track ,vector<PredictResult> & predict)
    {
        //同safety 多加一个变道
        //向相反方向补线 当偏差为0的时候可以停止

        PredictResult vehicle;
        vehicle.type=LABEL_NONE;
        switch(step)
        {
        //判断小车是否到达目标距离
        case STEP::EnterWait:
            vehicle = get_vehicle(predict);
            if((vehicle.y+vehicle.height)>ENTER_WAIT)
            {
                step=STEP::Enter;
            }
        break;
        //（1）根据小车的位置补线（2）判断小车是否到达一定位置
        case STEP::Enter:
            vehicle = get_vehicle(predict);
            //补线
            racing_add_line(track,vehicle);
            if((vehicle.x+vehicle.height)>ENTER)//
            {
                step=STEP::ExitWait;
                if(vehicle.type!=LABEL_NONE)last_vehicle=vehicle;        
            }
        break;
        //（1）补线（2）如果小车消失多帧则判断结束（3）判断结束之后开始定时
        case STEP::ExitWait:
            vehicle = get_vehicle(predict);
            if(vehicle.type==LABEL_NONE&&vehicle.type==LABEL_NONE)
            {
                //等于这个就相当于丢图
                lose++;
                if(lose>RACING_EXIT)
                {
                    step=STEP::Exit;
                    preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                    startTime=preTime;  
                    if(vehicle.type!=LABEL_NONE)last_vehicle=vehicle;    
                    step=STEP::Exit;
                    break;
                }
            }
            if(vehicle.type!=LABEL_NONE)last_vehicle=vehicle;    
            //补线
            racing_add_line(track,vehicle);            
        break;
        //（1）补线（2）当定时到达目标的时候结束
        case STEP::Exit:
            racing_add_line(track,last_vehicle); 
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            if((startTime - preTime)>EXIT_DELAY)
            {step=STEP::Change;}
        break;
        //相反方向补线 当偏差值小于一定值的时候进下一阶段
        case STEP::Change:
            spy_racing_add_line(track);
            bias.get_devation(track);
            if(abs(bias.devation)<BIAS)//bias不是绝对值
            {
                step=STEP::Wait;
                preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                startTime=preTime;  
            }
        break;
        //延时一定的时间等待小车车到达自己的位置
        case STEP::Wait:
            //如果定时达标退出
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            if((startTime - preTime)>WAIT_DELAY)
            {   
                init();
                return true;
            }
        break;
        }
        return false;
        
    }
    //danger    撞击
    int danger_peocess(track & track ,vector<PredictResult> & predict)
    {
        //不延时的safety        
        PredictResult vehicle;
        switch(step)
        {
        //判断小车是否到达目标距离
        case STEP::EnterWait:
            vehicle = get_vehicle(predict);
            danger_racing_add_line(track);
            // direction_judgment(track,vehicle);
            if((vehicle.y+vehicle.height)>ENTER_WAIT)
            {
                step=STEP::Enter;
            }
        break;
        //（1）根据小车的位置补线（2）判断小车是否到达一定位置
        case STEP::Enter:
            vehicle = get_vehicle(predict);
            //补线
            danger_racing_add_line(track);
            if((vehicle.x+vehicle.height)>ENTER)//
            {
                step=STEP::ExitWait;        
            }
            if(vehicle.type!=LABEL_NONE){last_vehicle=vehicle;}
        break;
        //（1）补线（2）如果小车消失多帧则判断结束（3）判断结束之后开始定时
        case STEP::ExitWait:
            vehicle = get_vehicle(predict);
            //补线
            danger_racing_add_line(track);    
            if(vehicle.type==LABEL_NONE&&vehicle.type==LABEL_NONE)
            {
                //等于这个就相当于丢图
                lose++;
                if(lose>RACING_EXIT)
                {
                    preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                    startTime=preTime;  
                    step=STEP::Exit;
                    break;
                }
            }
            if(vehicle.type!=LABEL_NONE){last_vehicle=vehicle;}            
        break;
        //（1）补线（2）当定时到达目标的时候结束
        case STEP::Exit:
            danger_racing_add_line(track); 
            startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
            if((startTime - preTime)>EXIT_DELAY)
            {
                init();
                return true;
            }
        break;
        break;
        }
        return false;
    }

    void draw(Mat & binary_mat)
    {
            putText(binary_mat,"racing step:"+to_string(step) , Point(COLSIMAGE/3,15),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
            switch(vehicle_type)
            {
                case LABEL_SAFETY:
                    putText(binary_mat,"LABEL_SAFETY", Point(COLSIMAGE/3,35),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
                break;
                case LABEL_SPY:
                    putText(binary_mat,"LABEL_SPY", Point(COLSIMAGE/3,35),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
                break;
                case LABEL_DANGER:
                    putText(binary_mat,"LABEL_DANGER", Point(COLSIMAGE/3,35),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
                break;
                case LABEL_PROP:
                    putText(binary_mat,"LABEL_PROP", Point(COLSIMAGE/3,35),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
                break;
            }
            putText(binary_mat,to_string(direction), Point(COLSIMAGE/3,55),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
    }


};